#!/usr/bin/python

"""
pyode-tutorial-2.py
This is an implementation of the PyODE tutorial 2 program using ode3d.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009-2012

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 3 of
the License, or (at your option) any later version. The text of the
GNU Lesser General Public License is included with this library in
the file named LICENSE.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

Note: This python example follows the tutorial2.py program distributed with
      the PyODE library.

      The original PyODE tutorial can be found here:
          http://pyOdesourceforge.net/tutorials/tutorial2.html

For reference here is the original program implemented using PyODE and pygame:

# pyODE example 2: Connecting bodies with joints

import pygame
from pygame.locals import *
import ode


def coord(x,y):
    "Convert world coordinates to pixel coordinates."
    return 320+170*x, 400-170*y


# Initialize pygame
pygame.init()

# Open a display
srf = pygame.display.set_mode((640,480))

# Create a world object
world = OdeWorld()
world.setGravity((0,-9.81,0))

# Create two bodies
body1 = OdeBody(world)
M = OdeMass()
M.setSphere(2500, 0.05)
body1.setMass(M)
body1.setPosition((1,2,0))

body2 = OdeBody(world)
M = OdeMass()
M.setSphere(2500, 0.05)
body2.setMass(M)
body2.setPosition((2,2,0))

# Connect body1 with the static environment
j1 = OdeBallJoint(world)
j1.attach(body1, Odeenvironment)
j1.setAnchor( (0,2,0) )

# Connect body2 with body1
j2 = OdeBallJoint(world)
j2.attach(body1, body2)
j2.setAnchor( (1,2,0) )


# Simulation loop...

fps = 50
dt = 1.0/fps
loopFlag = True
clk = pygame.time.Clock()

while loopFlag:
    events = pygame.event.get()
    for e in events:
        if e.type==QUIT:
            loopFlag=False
        if e.type==KEYDOWN:
            loopFlag=False

    # Clear the screen
    srf.fill((255,255,255))

    # Draw the two bodies
    x1,y1,z1 = body1.getPosition()
    x2,y2,z2 = body2.getPosition()
    pygame.draw.circle(srf, (55,0,200), coord(x1,y1), 20, 0)
    pygame.draw.line(srf, (55,0,200), coord(0,2), coord(x1,y1), 2)
    pygame.draw.circle(srf, (55,0,200), coord(x2,y2), 20, 0)
    pygame.draw.line(srf, (55,0,200), coord(x1,y1), coord(x2,y2), 2)

    pygame.display.flip()

    # Next simulation step
    world.step(dt)

    # Try to keep the specified framerate    
    clk.tick(fps)
"""

from pandac.PandaModules import OdeBallJoint, Mat4

import ode3d
import pvis

# Create a ode3d world object
world = ode3d.World()
world.setGravity((0,-9.81,0))

# Set the pvis scene so we won't have auto-scaling goofing us up.
#world.scene.autoscale = False
#world.scene.center = (0, 1, 0)
#world.scene.range = 2.5
pvis.base.disableMouse()
pvis.base.camera.setPos(0, 2, 7)
pvis.base.camera.lookAt(0, 1, 0) #@UndefinedVariable -- lookAt(), more Panda3d magic?
pvis.base.camera.setR(180) #@UndefinedVariable -- setR(), more Panda3d magic?

# Turn mouse control back on
mat=Mat4(pvis.base.camera.getMat()) #@UndefinedVariable -- getMat(), more Panda3d magic?
mat.invertInPlace()
pvis.base.mouseInterfaceNode.setMat(mat)
pvis.base.enableMouse()

# Create an axis reference, x = red, y = green, z = blue
pvis.arrow(pos=(0,0,0), q=pvis.xAxis, shaftwidth=0.1, color=(1,0,0), length=0.5)
pvis.arrow(pos=(0,0,0), q=pvis.yAxis, shaftwidth=0.1, color=(0,1,0), length=0.5)
pvis.arrow(pos=(0,0,0), q=pvis.zAxis, shaftwidth=0.1, color=(0,0,1), length=0.5)

# Create the first sphere
body1 = ode3d.GDMBody(world)
body1.AddElement('Sphere', ode3d.GDMElement().DefineSphere(2500, 0.15)) # density, radius

body1.AddElement('rod', ode3d.DisplayElement(pvis.arrow(pos=(0,0,0), q=pvis.xAxisInv, shaftwidth=0.02))) # For the suspending rod
body1.setPosition((1, 2, 0))

# Create the second sphere
body2 = ode3d.GDMBody(world)
body2.AddElement('Sphere', ode3d.GDMElement().DefineSphere(2500, 0.15)) # density, radius
body2.AddElement('rod', ode3d.DisplayElement(pvis.arrow(pos=(0,0,0), q=pvis.xAxisInv, shaftwidth=0.02))) # For the suspending rod
body2.setPosition((2, 2, 0))

# Create attachment points (in global coordinates).
body1.DefineConnectionPoint('Anchor1', (0,2,0))
body1.DefineConnectionPoint('Anchor2', (1,2,0))
body2.DefineConnectionPoint('Anchor1', (1,2,0))

# Create joints and make the connections
body1.Connect(OdeBallJoint(world), 'Anchor1', None, None) # Connect to the environment
body2.Connect(OdeBallJoint(world), 'Anchor1', body1, 'Anchor2')

# Simulation loop...
fps = 50
steps = int(fps * 0.02)
dt = 1.0 / fps / steps
pvis.rate(fps) # Try to keep the specified framerate
#world.scene.range.x = 10

def loop(task):
    for n in range(steps):
        world.step(dt)

    body1.UpdateDisplay()
    body2.UpdateDisplay()

    return task.cont

pvis.Run(loop)